
#include "ganimator.h"

GAnimator::GAnimator()
   :Animator() {
   filter = 0;
}

GAnimator::GAnimator(const GAnimator& anim) 
   :Animator(anim.skeleton, anim.motion) {
   
   cout<<"GAnimator copy constructor!!!!"<<endl;
   gskeleton = anim.gskeleton;
   ijoints = anim.ijoints;
   
   filter = 0;
   setting();
   /// call the setting method??????
}

GAnimator::GAnimator(GSkeleton *gskel, Skeleton *skeleton, Motion *motion)
   :Animator(skeleton, motion) {
   
   gskeleton = gskel;
   // filter will be initialized on setting method
   filter = 0;
   setting();
}

void GAnimator::setting() {
   
   assert(gskeleton != 0);
   assert(skeleton != 0);
   
   cout<<"GAnimator::setting() "<<endl;
   /// FILTERING SETTINGS
   // 1. Build the mapping between the two joints set
   jco.resize(skeleton->nBones()+1);
   jco[0] = 6;   jco[1] = 19;   jco[2] = 12;   jco[3] = 14;   jco[4] = 11;
   jco[5] = 20;  jco[6] = 2;    jco[7] = 4;    jco[8] = 21;   jco[9] = 13;
   jco[10] = 22; jco[11] = 15;  jco[12] = 8;   jco[13] = 10;  jco[14] = 7;
   jco[15] = 16; jco[16] = 17;  jco[17] = 9;   jco[18] = 18;  jco[19] = 0;
   jco[20] = 1;  jco[21] = 5;   jco[22] = 3;
   
   jcoI.resize(jco.size());
   for (int i=0; i<jcoI.size(); ++i) 
      jcoI[jco[i]] = i;
   
   // 2. Builds the modified prev list
   const vector<int> &prev = gskeleton->prev;
   mprev.resize(jco.size());
   for (int i=0; i<mprev.size(); ++i) 
      if (prev[i] == -1) mprev[jco[i]] = -1;
      else               mprev[jco[i]] = jco[prev[i]];
   
   // 3. Build the modified joints 
   ijoints.resize(jco.size());
   mjoints.resize(jco.size());
   for (int i=0; i<mjoints.size(); ++i) {
      const Point3 &p = gskeleton->getJointPos(i);
      ijoints[i] = p;
      mjoints[jco[i]] = OMR::Vector3(p[0], p[1], p[2]);
   }
   
   gskeleton->transformation.resize(gskeleton->nBones());
   
   #if 1
   cout<<"the original joints"<<endl;
   for (int i=0; i<ijoints.size(); ++i) 
      cout<<i<<" : "<<ijoints[i]<<endl;
   
   cout<<"the modified joints"<<endl;
   for (int i=0; i<mjoints.size(); ++i) 
      cout<<i<<" : "<<mjoints[i]<<endl;
   
   cout<<"the modified prev"<<endl;
   for (int i=0; i<mprev.size(); ++i) 
      cout<<i<<" : "<<mprev[i]<<endl;
   
   cout<<"the corres array"<<endl;
   for (int i=0; i<jco.size(); ++i) 
      cout<<i<<" : "<<jco[i]<<endl;
   #endif
   
   // 4. Compute the legRatio
   Point3 source0 = skeleton->getJointPos(13);
   Point3 source1 = skeleton->getJointPos(22);
   Number difSource = abs(source0[1] - source1[1]);
   legRatio = abs(ijoints[13][1] - ijoints[22][1])/difSource;
   cout<<"legRatio: "<<legRatio<<endl;
   
   filter = new OMR::MotionFilter(mjoints, mprev);
}

void GAnimator::frame(int f) {
   
   /// Animate the skeleton
   Animator::frame(f);
   
   vector<int> bco(gskeleton->nBones()); // tabla de correspondencia de bones
   
   /// Transformations filtering
   // 1. get the transformations
   vector<OMR::Transform<> > transfInput(skeleton->nBones());
   for (int i=0; i<skeleton->nBones(); ++i) { // Get the bones transformations
      const pair<int,int> &b = gskeleton->bones[i]; 
      Quat Q = skeleton->getBoneQuaternion(i); /// source
      int ii = jco[b.first] - 1;
      Vector3 t(0,0,0);
      if (ii == 0) 
         t = skeleton->getJointPos(b.first) - skeletonIni->getJointPos(b.first);
      
      OMR::Quaternion<> TQ(Q[0],Q[1],Q[2],Q[3]); 
      OMR::Vector3      Tt(t[0],t[1],t[2]);
      OMR::Transform<>  T(TQ, 1.0, Tt);
      transfInput[ii] = T;
      bco[i] = ii;
   }
   
   vector<int> bcoI(skeleton->nBones());
   for (int i=0; i<bcoI.size(); ++i) 
      bcoI[bco[i]] = i;
   
   // 2. get the feet
   vector<OMR::Vector3> feet;
   Point3 f0 = skeleton->getJointPos(13);
   Point3 f1 = skeleton->getJointPos(22);
   Point3 f2 = skeleton->getJointPos(3);
   feet.push_back(OMR::Vector3(f0[0]*legRatio,f0[1]*legRatio,f0[2]*legRatio));
   feet.push_back(OMR::Vector3(f1[0]*legRatio,f1[1]*legRatio,f1[2]*legRatio));
   feet.push_back(OMR::Vector3(f2[0]*legRatio,f2[1]*legRatio,f2[2]*legRatio));
   
   // 3. transformations filtering
   filter->step(transfInput, feet);
   
   // 4. Apply the transformations (animate)
   const vector<OMR::Transform<> > &Tl = filter->getTransforms();
   const OMR::Transform<> &T0 = Tl[0];
   OMR::Vector3 p = mjoints[0];
   p = T0*p;
   gskeleton->setJointPos(jcoI[0], Point3(p[0],p[1],p[2]));
   for (int i=0; i<Tl.size(); ++i) {
      //~ const pair<Number, int> &param = gskeleton->getRelatedParamBone(i);
      //~ const int &idbone = param.second; // id of corresponding bone on source skeleton
      
      OMR::Vector3 joint = Tl[i]*mjoints[i+1];
      gskeleton->setJointPos(jcoI[i+1], Point3(joint[0],joint[1],joint[2]));
      gskeleton->transformation[bcoI[i]] = Tl[i];
   }
   
   /// cuidado!!! esto solo funciona porque la numeracion de los bones responde
   /// a un orden donde los ultimos del target son los que no tienen 
   /// correspondientes en el source 
   for (int i=skeleton->nBones(); i<gskeleton->nBones(); ++i)
      gskeleton->transformation[i] = gskeleton->transformation[skeleton->nBones()-1];
}
